# -*- coding: utf-8 -*-
#
# Copyright (C) 2024 shenzhen reinovo, Inc. All Rights Reserved 
#
# @Time    : 2024/12/05 下午3:26
# @Author  : hmm
# @Email   : liuyuhang0531@foxmail.com
# @File    : main.py


from flask import Flask, render_template, request,jsonify
from flask_socketio import SocketIO, emit,disconnect
from rei_roslaunch import robot_launch,ReiRoslaunch
from multiprocessing import Queue
from sensor_msgs.msg import LaserScan
import rospy
import glob
import os
import time
import signal

app = Flask(import_name=__name__,
            static_folder='static',
            template_folder='templates')
# 防止把中文转换为ASCII编码
app.config['JSON_AS_ASCII'] = False
# 设置一个秘钥
app.config['SECRET_KEY'] = 'secret!'
socketio = SocketIO(app)

logData = Queue()
logHistory = []
codeHistory = []
navGoalHistory = {}

class robot_startup():
    def __init__(self,rei_robot=None):
        if rei_robot == None:
            try:
                self.rei_robot = os.environ['REI_ROBOT']
            except:
                print('未找到设备信息，请自定义')
        else:
            self.rei_robot = rei_robot
        self.log_level = ['INFO','WARN','ERROR']
        self.cfg = self._read_json_file()
        self.devList = os.popen('ls /dev').read().split('\n')
        self.runList = []

    def pub_log(self,text,level=0):
        json = {'time':str(time.asctime()),
                'level':self.log_level[level],
                'msg':text}
        print(json)
        logData.put(json)

    def _read_json_file(self):
        import json
        try:
            with open('../src/config/reirobot.json', 'r') as file:
                data = json.load(file)
                return data
        except FileNotFoundError:
            print("config/reirobot.json:文件未找到")
        except json.JSONDecodeError:
            print("config/reirobot.json:文件JSON数据解析错误")
        return data

    def open_navigation(self,map_name):
        self.shutdown()        
        launchlist = []
        nav = self.cfg['robotConfig'][self.rei_robot]['nav']
        nav['args'].append('map_file_name:='+map_name)
        launchlist.append({'pkg':'rosbridge_server','file':'rosbridge_websocket.launch','args':[]},)
        launchlist.append(nav)
        launchlist.append({'pkg':'relative_move',
                           'file':'relative_move.launch',
                           'args':[]})
        if 'base_camera' in self.devList:
            launchlist.append({'pkg':'ar_pose',
                               'file':'ar_base.launch',
                               'args':[]})
        else:
            self.pub_log('未检测到底盘相机',1)
        # if self.devList == 'hand_camera':
        #     launchlist.append({'pkg':'ar_pose',
        #                        'file':'ar_base',
        #                        'args':[]})
        #     if self.devList == 'ttyACM0':
        #         self.pub_log('检测到机械臂')
        #         launchlist.append({'pkg':'arm_controller',
        #                         'file':'pick_ar.launch',
        #                         'args':[]})
        # else:
        #     if self.devList == 'ttyACM0':
        #         self.pub_log('检测到机械臂')
        #         launchlist.append({'pkg':'arm_controller',
        #                         'file':'arm_controller.launch',
        #                         'args':[]})

        ros_nav = ReiRoslaunch(launchlist,is_core=True)
        ros_nav.start()
        self.pub_log('rosbridge_server启动成功')
        ros_nav.start_node('tf2_web_republisher','tf2_web_republisher','tf2_web_republisher')
        self.runList.append(ros_nav)
        self.pub_log('tf2_web启动成功')
        return True
    
    def open_slam(self):
        self.shutdown()
        launchlist = []
        slam = self.cfg['robotConfig'][self.rei_robot]['slam']
        launchlist.append({'pkg':'rosbridge_server','file':'rosbridge_websocket.launch','args':[]},)
        launchlist.append(slam)
        ros_slam = ReiRoslaunch(launchlist,is_core=True)
        ros_slam.start()
        self.pub_log('地图构建启动成功')
        ros_slam.start_node('tf2_web_republisher','tf2_web_republisher','tf2_web_republisher')
        self.pub_log('tf2_web启动成功')
        self.runList.append(ros_slam)

    def save_map(self,map_name):
        self.pub_log('保存地图中')
        launch = self.cfg['robotConfig'][self.rei_robot]['map_save']
        launch['args'].append('map_file:='+map_name)
        ros = ReiRoslaunch([launch])
        ros.start()
        self.runList.append(ros)

    def shutdown(self):
        for ros in self.runList:
            if ros.getState() == "ACTIVE":
                ros.shutdown()
        self.runList = []
        self.pub_log('roscore已经关闭')
        return False

    def getState(self):
        for ros in self.runList:
            return ros.getState

ros = robot_startup()

@socketio.on('reinovo_log')
def log_info(cmd): 
    data = logData.get()
    emit('reinovo_response', data)

@socketio.on('RobotData')
def RobotData(History):
    for key in History.keys():
        print(key)
        if key == 'log':
            logHistory.insert(0,History['log']) #str(time.asctime()) + ' [远程端] ' + 
        elif key == 'code':
            codeHistory.insert(0,History['code'])
            print(History['code'])
        elif key == 'goal':
            navGoalHistory[History['goal'][0]] = History['goal'][1]
        elif key == 'delGoal':
            del navGoalHistory[History['delGoal']]
    data = {}
    try:
        robot_ID = os.environ['REI_ROBOT']
        # 获取地图目录
        mapDir = os.environ["MAP_DIRECTORY"]
        # 获取人脸数据目录
        faceDir = os.environ["FACE_DIRECTORY"]
        data['robot_ID'] = robot_ID
        data['maps_list'] = [file.split('/')[-1][:-4] for file in glob.glob(mapDir+'/*.pgm')]
        data['pose_list'] = navGoalHistory
        data['face_data_list'] = [face for face in os.listdir(faceDir)]
        data['log'] = logHistory
        data['code'] = codeHistory
    except KeyError as err:
        if err == "MAP_DIRECTORY":
            ros.pub_log('获取地图目录失败',1)
        elif err == "FACE_DIRECTORY":
            ros.pub_log('获取人脸数据目录失败')
        elif err == "REI_ROBOT":
            ros.pub_log('未识别到机器人型号')
    time.sleep(1)
    emit('RobotData_response', data)

@app.route('/')
def index_page():
    ros.shutdown()
    return render_template('index.html')

@app.route('/blockly')
def blackly_page():
    return render_template('blockly-code.html')

@app.route('/slam')
def slam_page():
    return render_template('slam.html')

@app.route('/startSlam', methods=['POST'])
def start_slam():
    dst = {"result":False,
           "mssage":""}
    if request.method == "POST":
        cmd = request.form.get('cmd')
        if cmd == "open":
            ros.pub_log('地图构建请求')
            ros.open_slam()
            dst['result'] = True
            dst['mssage'] = 'startup successfully'
            ros.pub_log('地图构建开启成功')
            rospy.init_node("rei_web_node",disable_signals=True,anonymous=True)
            ros.pub_log('等待雷达开启')
            try:
                rospy.wait_for_message('/fuse_scan',LaserScan,timeout=10)
                ros.pub_log('雷达开启成功')
                return jsonify(dst)
            except rospy.exceptions.ROSException as err:
                ros.pub_log('雷达开启失败',2)
                ros.shutdown()
        elif cmd == 'close':
            ros.pub_log('建图关闭请求')
            ros.shutdown()
            dst['result'] = True
            dst['mssage'] = 'Closed successfully'
    return jsonify(dst)

@app.route('/saveSlam', methods=['POST'])
def save_slam():
    dst = {"result":False,
           "mssage":""}
    if request.method == "POST":
        cmd = request.form.get('cmd')
        if cmd == "open":
            map_name = request.form.get('map_name')
            print('---------------'+map_name+'---------------')
            ros.pub_log('地图保存请求')
            ros.save_map(map_name)
            dst['result'] = True
            dst['mssage'] = 'map save successfully'
            ros.pub_log('地图保存开启成功')
    return jsonify(dst)

@app.route('/startNav', methods=['POST'])
def start_nav():
    dst = {"result":False, 
           "mssage":""}
    if request.method == "POST":
        cmd = request.form.get('cmd')
        if cmd == 'open':
            ros.pub_log('导航启动请求')
            map_name = request.form.get('map_name')
            ros.open_navigation(map_name)
            dst['result'] = True
            dst['mssage'] = 'startup successfully'
            ros.pub_log('导航开启成功')
            rospy.init_node("rei_web_node",disable_signals=True,anonymous=True)
            ros.pub_log('等待雷达开启')
            try:
                rospy.wait_for_message('/fuse_scan',LaserScan,timeout=10)
                ros.pub_log('雷达开启成功')
                return jsonify(dst)
            except rospy.exceptions.ROSException as err:
                ros.pub_log('雷达开启失败',2)
                ros.shutdown()
        elif cmd == 'close':
            ros.pub_log('导航关闭请求')
            ros.shutdown()
            dst['result'] = True
            dst['mssage'] = 'Closed successfully'
    return jsonify(dst)

if __name__ == '__main__':
    socketio.run(app, host='192.168.8.1',port=5000)
    ros.shutdown()
    os.kill(os.getpid(), signal.SIGTERM)